#include <iostream>
#include <string>
#include <fstream>
#include <deque>
#include <thread>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

// using namespace std;


class ProcessTopicsMsg
{
public:
    ros::NodeHandle nh;
    ros::Subscriber subLegOdom;
    ros::Subscriber subLidarOdom;

    ros::Publisher pubOdomForward;
    ros::Publisher pubOdomPath;
    ros::Publisher pubAftLegOdom;
    ros::Publisher pubAftLegOdomPath;

    std::mutex odomLegLock;
    std::mutex odomLidarLock;
    std::deque<nav_msgs::Odometry> legOdomQue;
    std::deque<nav_msgs::Odometry> lidarOdomQue;

    // raw legodom
    nav_msgs::Odometry odomProcess;
    nav_msgs::Path odomPath;

    // 时间对齐的legodom
    nav_msgs::Odometry deskewLegOdomMsg;
    nav_msgs::Path deskewLegOdomMsgPath;

    float transform[7] = {0, 0, 0, 0, 0, 0, 0};
    double legodom_time = 0;
    double lidar_end_time = 0;
    double now_time = 0;
    double last_timestamp_legodom = -1;
    bool deskewAvailable = false;

    ProcessTopicsMsg()
    {
        subLegOdom      = nh.subscribe<nav_msgs::Odometry>("/leg_odom", 1000, &ProcessTopicsMsg::legOdometry_handle, this, ros::TransportHints().tcpNoDelay());
        subLidarOdom    = nh.subscribe<nav_msgs::Odometry>("/Odometry", 1000, &ProcessTopicsMsg::lidarOdometry_handle, this, ros::TransportHints().tcpNoDelay());
                                    
        pubOdomForward  = nh.advertise<nav_msgs::Odometry>("/forwardOdom", 10000, true);
        pubOdomPath     = nh.advertise<nav_msgs::Path>("/path_legodom", 10000, true);
        pubAftLegOdom   = nh.advertise<nav_msgs::Odometry>("/aft_odom_legodom", 10000, true);
        pubAftLegOdomPath = nh.advertise<nav_msgs::Path>("/aft_path_legodom", 10000, true);
    }

    void legOdometry_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        std::lock_guard<std::mutex> lock1(odomLegLock);
        nav_msgs::Odometry data = *msgIn;
        now_time = ros::Time::now().toSec();
        // data.header.stamp = ros::Time::now();

        legOdomQue.push_back(data);

        legodom_time = data.header.stamp.toSec();
        last_timestamp_legodom = data.header.stamp.toSec();

        transform[0] = data.pose.pose.position.x;
        transform[1] = data.pose.pose.position.y;
        transform[2] = data.pose.pose.position.z;
        transform[3] = data.pose.pose.orientation.x;
        transform[4] = data.pose.pose.orientation.y;
        transform[5] = data.pose.pose.orientation.z;
        transform[6] = data.pose.pose.orientation.w;

        // ROS_INFO("\033[1;32m legodom_time: %f; now_time: %f; \033[0m", legodom_time, now_time);
        // ROS_INFO("\033[1;31m x: %f; y: %f;  z: %f;\033[0m", transform[0], transform[1], transform[2]);

        publish_odometry(pubOdomForward);   // 发布订阅修订坐标系的legodom Odom
        publish_path(pubOdomPath);          // 发布订阅修订坐标系的legodom Path    
    }

    void lidarOdometry_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        std::lock_guard<std::mutex> lock2(odomLidarLock);
        nav_msgs::Odometry data = *msgIn;      
        // data.header.stamp = ros::Time::now();

        lidarOdomQue.push_back(data);

        deskewAvailable = true;

        lidar_end_time = data.header.stamp.toSec();
        // ROS_INFO("\033[1;32m lidar_end_time: %f; \033[0m", lidar_end_time);

    }




    void processLegOdomMsg()
    {
        ros::Rate rate(5000);
        while (ros::ok())
        {
            deskewInfo_1();

            rate.sleep();
        }
    }


    


    /***************************************************
     * @brief 第一种逻辑方式进行插补的程序
     * 简介逻辑：
     * 1.进行时间同步处理
     * 2.进行插补
     * 3.发布相关信息
     **************************************************/
    void deskewInfo_1()
    {
        double timeFrom, timeTo;
        nav_msgs::Odometry legOdomFrom, legOdomTo;

        if (deskewAvailable)
        {
            if (sync_packages())
            {
                for (int i = 0; i < (int)legOdomQue.size(); ++i)
                {
                    nav_msgs::Odometry thisLegOdomMsg = legOdomQue[i];
                    double currentLegOdomTime = thisLegOdomMsg.header.stamp.toSec();
                    if (currentLegOdomTime < lidar_end_time)
                    {
                        timeFrom = currentLegOdomTime;
                        legOdomFrom = thisLegOdomMsg;
                        continue;
                    }
                    else
                    {
                        timeTo = currentLegOdomTime;
                        legOdomTo = thisLegOdomMsg;
                        break;
                    }
                }

                double ratioFront, ratioBack;
                ratioFront = (lidar_end_time - timeFrom) / (timeTo - timeFrom); // 前向部分的所占比率
                ratioBack = (timeTo - lidar_end_time) / (timeTo - timeFrom);    // 后向部分的所占比率
                ROS_INFO("\033[1;31m ratioFront: %f; ratioBack: %f; \033[0m", ratioFront, ratioBack);
            
                // 四元数球面插值
                tf::Quaternion qFront, qBack, qCur;
                tf::quaternionMsgToTF(legOdomFrom.pose.pose.orientation, qFront);
                tf::quaternionMsgToTF(legOdomTo.pose.pose.orientation, qBack);
                qCur = qFront.slerp(qBack, ratioFront);

                // 位置线性插值
                tf::Point pFront, pBack, pCur;
                tf::pointMsgToTF(legOdomFrom.pose.pose.position, pFront);
                tf::pointMsgToTF(legOdomTo.pose.pose.position, pBack);
                pCur = pFront.lerp(pBack, ratioFront);

                // 赋值时间对齐后的数据
                tf::quaternionTFToMsg(qCur, deskewLegOdomMsg.pose.pose.orientation);
                tf::pointTFToMsg(pCur, deskewLegOdomMsg.pose.pose.position);
                deskewAvailable = false;

                // Info of Quaternion input for debug
                ROS_INFO("\033[1;31m ratioFront: %f; ratioBack: %f; \033[0m", ratioFront, ratioBack);
                ROS_INFO("\033[1;32m timeFront: %f; timeBack: %f; \033[0m", lidar_end_time - timeFrom, timeTo - lidar_end_time);

                double Roll, Pitch, Yaw;
                tf::Matrix3x3(qFront).getRPY(Roll, Pitch, Yaw);
                ROS_INFO("\033[1;34m FRoll: %f; FPitch: %f; FYaw: %f; \033[0m", Roll, Pitch, Yaw);
                tf::Matrix3x3(qBack).getRPY(Roll, Pitch, Yaw);
                ROS_INFO("\033[1;36m BRoll: %f; BPitch: %f; BYaw: %f; \033[0m", Roll, Pitch, Yaw);
                tf::Matrix3x3(qCur).getRPY(Roll, Pitch, Yaw);
                ROS_INFO("\033[1;33m Roll: %f; Pitch: %f; Yaw: %f; \033[0m", Roll, Pitch, Yaw);

                ROS_INFO("\033[1;34m Fx: %f; Fy: %f; Fz: %f; \033[0m", pFront.x(), pFront.y(), pFront.z());
                ROS_INFO("\033[1;36m Bx: %f; By: %f; Bz: %f; \033[0m", pBack.x(), pBack.y(), pBack.z());
                ROS_INFO("\033[1;33m Cx: %f; Cy: %f; Cz: %f; \033[0m", pCur.x(), pCur.y(), pCur.z());
                ROS_INFO("\033[1;34m --------- \033[0m");

                publish_aftLegodom_Odom(pubAftLegOdom);     // 发布时间对齐后的legodom Odom
                publish_aftLegodom_Path(pubAftLegOdomPath); // 发布时间对齐后的legodom Path
            }   
        }
    }
    
    /***************************************************
     * @brief 进行legodom和lidar的时间同步对齐
     * 
     * @return true 完成
     * @return false 未完成
     **************************************************/
    bool sync_packages()
    {      
        if (legOdomQue.empty())
            return false;

        if (last_timestamp_legodom < lidar_end_time)
            return false;

        while (!legOdomQue.empty())
        {
            if (legOdomQue.front().header.stamp.toSec() < lidar_end_time - 0.05)
                legOdomQue.pop_front();
            else
                break;
        }
        return true;
    }




    /***************************************************
     * @brief 第二种逻辑方式进行插补的程序
     * 
     **************************************************/
    void deskewInfo_2()
    {       
        if (deskewAvailable)
        {
            std::lock_guard<std::mutex> lock1(odomLegLock);
            std::lock_guard<std::mutex> lock2(odomLidarLock);

            if (legOdomQue.empty() || legOdomQue.front().header.stamp.toSec() > lidar_end_time || legOdomQue.back().header.stamp.toSec() < lidar_end_time)
            {
                return;
            }

            legOdomDsekewInfo(&deskewLegOdomMsg);   // 对legodom进行时间对齐处理
            deskewAvailable = false;

            publish_aftLegodom_Odom(pubAftLegOdom);     // 发布时间对齐后的legodom Odom
            publish_aftLegodom_Path(pubAftLegOdomPath); // 发布时间对齐后的legodom Path

            return;
        }        
    }

    
    void legOdomDsekewInfo(nav_msgs::Odometry *curLegOdomMsg)
    {
        double timeFrom, timeTo;
        nav_msgs::Odometry legOdomFrom, legOdomTo;

        // 清空legOdomQue队列中位于lidar帧前0.05s的数据
        while (!legOdomQue.empty())
        {
            if (legOdomQue.front().header.stamp.toSec() < lidar_end_time - 0.05)
                legOdomQue.pop_front();
            else
                break;
        }

        if (legOdomQue.empty())
            return;
        
        for (int i = 0; i < (int)legOdomQue.size(); ++i)
        {
            nav_msgs::Odometry thisLegOdomMsg = legOdomQue[i];
            double currentLegOdomTime = thisLegOdomMsg.header.stamp.toSec();
            if (currentLegOdomTime < lidar_end_time)
            {
                timeFrom = currentLegOdomTime;
                legOdomFrom = thisLegOdomMsg;
                continue;
            }
            else
            {
                timeTo = currentLegOdomTime;
                legOdomTo = thisLegOdomMsg;
                break;
            }
        }

        double ratioFront, ratioBack;
        ratioFront = (lidar_end_time - timeFrom) / (timeTo - timeFrom); // 前向部分的所占比率
        ratioBack = (timeTo - lidar_end_time) / (timeTo - timeFrom);    // 后向部分的所占比率

        // ROS_INFO("\033[1;32m From: %f; lidar: %f; To: %f; \033[0m"
        //          "\033[1;36m Begin: %f; %f;\033[0m"
        //          "\033[1;34m End: %f; %f;\033[0m",
        //                     timeFrom,    lidar_end_time,     timeTo,
        //                     lidar_end_time - timeFrom,   ratioFront,
        //                     timeTo - lidar_end_time,     ratioBack);


        // 四元数球面插值
        tf::Quaternion qFront, qBack, qCur;
        tf::quaternionMsgToTF(legOdomFrom.pose.pose.orientation, qFront);
        tf::quaternionMsgToTF(legOdomTo.pose.pose.orientation, qBack);
        qCur = qFront.slerp(qBack, ratioFront);

        // 位置线性插值
        tf::Point pFront, pBack, pCur;
        tf::pointMsgToTF(legOdomFrom.pose.pose.position, pFront);
        tf::pointMsgToTF(legOdomTo.pose.pose.position, pBack);
        pCur = pFront.lerp(pBack, ratioFront);

        // 赋值时间对齐后的数据
        tf::quaternionTFToMsg(qCur, curLegOdomMsg->pose.pose.orientation);
        tf::pointTFToMsg(pCur, curLegOdomMsg->pose.pose.position);



        // Info of Quaternion input for debug
        ROS_INFO("\033[1;31m ratioFront: %f; ratioBack: %f; \033[0m", ratioFront, ratioBack);
        ROS_INFO("\033[1;32m timeFront: %f; timeBack: %f; \033[0m", lidar_end_time - timeFrom, timeTo - lidar_end_time);

        double Roll, Pitch, Yaw;
        tf::Matrix3x3(qFront).getRPY(Roll, Pitch, Yaw);
        ROS_INFO("\033[1;34m FRoll: %f; FPitch: %f; FYaw: %f; \033[0m", Roll, Pitch, Yaw);
        tf::Matrix3x3(qBack).getRPY(Roll, Pitch, Yaw);
        ROS_INFO("\033[1;36m BRoll: %f; BPitch: %f; BYaw: %f; \033[0m", Roll, Pitch, Yaw);
        tf::Matrix3x3(qCur).getRPY(Roll, Pitch, Yaw);
        ROS_INFO("\033[1;33m Roll: %f; Pitch: %f; Yaw: %f; \033[0m", Roll, Pitch, Yaw);

        ROS_INFO("\033[1;34m Fx: %f; Fy: %f; Fz: %f; \033[0m", pFront.x(), pFront.y(), pFront.z());
        ROS_INFO("\033[1;36m Bx: %f; By: %f; Bz: %f; \033[0m", pBack.x(), pBack.y(), pBack.z());
        ROS_INFO("\033[1;33m Cx: %f; Cy: %f; Cz: %f; \033[0m", pCur.x(), pCur.y(), pCur.z());
        ROS_INFO("\033[1;34m --------- \033[0m");
    }








    /***************************************************
     * @brief 发布legodom，坐标系转换成 "camera_init"
     * 
     **************************************************/
    void publish_odometry(const ros::Publisher &pubOdomProcess)
    {
        odomProcess.header.frame_id = "camera_init";
        odomProcess.child_frame_id = "legodom";
        odomProcess.header.stamp = ros::Time().fromSec(legodom_time);
        odomProcess.pose.pose.position.x = transform[0];
        odomProcess.pose.pose.position.y = transform[1];
        odomProcess.pose.pose.position.z = transform[2];
        odomProcess.pose.pose.orientation.x = transform[3];
        odomProcess.pose.pose.orientation.y = transform[4];
        odomProcess.pose.pose.orientation.z = transform[5];
        odomProcess.pose.pose.orientation.w = transform[6];
        pubOdomProcess.publish(odomProcess);

        // 广播发布TF变换关系
        static tf::TransformBroadcaster br;
        tf::Transform tran;
        tf::Quaternion q;
        tran.setOrigin(tf::Vector3(odomProcess.pose.pose.position.x,
                                   odomProcess.pose.pose.position.y,
                                   odomProcess.pose.pose.position.z));
        q.setX(odomProcess.pose.pose.orientation.x);
        q.setY(odomProcess.pose.pose.orientation.y);
        q.setZ(odomProcess.pose.pose.orientation.z);
        q.setW(odomProcess.pose.pose.orientation.w);
        tran.setRotation(q);
        br.sendTransform(tf::StampedTransform(tran, odomProcess.header.stamp, "camera_init", "legodom"));
    }

    /***************************************************
     * @brief 发布原始的legodom path里程计
     * 
     **************************************************/
    void publish_path(const ros::Publisher pubPath)
    {
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header.stamp = ros::Time().fromSec(legodom_time);
        pose_stamped.header.frame_id = "camera_init";

        pose_stamped.pose.position.x = transform[0];
        pose_stamped.pose.position.y = transform[1];
        pose_stamped.pose.position.z = transform[2];
        pose_stamped.pose.orientation.x = transform[3];
        pose_stamped.pose.orientation.y = transform[4];
        pose_stamped.pose.orientation.z = transform[5];
        pose_stamped.pose.orientation.w = transform[6];    

        odomPath.header.frame_id = "camera_init";
        odomPath.header.stamp = ros::Time().fromSec(legodom_time);
        odomPath.poses.push_back(pose_stamped);
        pubPath.publish(odomPath);
    } 






    /***************************************************
     * @brief 发布时间对齐后的legodom odom
     * 
     **************************************************/
    void publish_aftLegodom_Odom(const ros::Publisher &pubOdom)
    {
        deskewLegOdomMsg.header.stamp = ros::Time().fromSec(lidar_end_time);
        deskewLegOdomMsg.header.frame_id =  "camera_init";
        deskewLegOdomMsg.child_frame_id = "aft_legodom";

        pubOdom.publish(deskewLegOdomMsg);


        // 广播发布TF变换关系，时间对齐后的lego_odom
        static tf::TransformBroadcaster br;
        tf::Transform tran;
        tf::Quaternion q;
        tran.setOrigin(tf::Vector3(deskewLegOdomMsg.pose.pose.position.x,
                                   deskewLegOdomMsg.pose.pose.position.y,
                                   deskewLegOdomMsg.pose.pose.position.z));
        q.setX(deskewLegOdomMsg.pose.pose.orientation.x);
        q.setY(deskewLegOdomMsg.pose.pose.orientation.y);
        q.setZ(deskewLegOdomMsg.pose.pose.orientation.z);
        q.setW(deskewLegOdomMsg.pose.pose.orientation.w);
        tran.setRotation(q);
        br.sendTransform(tf::StampedTransform(tran, deskewLegOdomMsg.header.stamp, "camera_init", "aft_legodom"));


    }
    
    /***************************************************
     * @brief 发布时间对齐后的legodom path
     * 
     **************************************************/
    void publish_aftLegodom_Path(const ros::Publisher pubPath)
    {
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header.stamp = ros::Time().fromSec(lidar_end_time);
        pose_stamped.header.frame_id = "camera_init";

        pose_stamped.pose.position.x = deskewLegOdomMsg.pose.pose.position.x;
        pose_stamped.pose.position.y = deskewLegOdomMsg.pose.pose.position.y;
        pose_stamped.pose.position.z = deskewLegOdomMsg.pose.pose.position.z;
        pose_stamped.pose.orientation.x = deskewLegOdomMsg.pose.pose.orientation.x;
        pose_stamped.pose.orientation.y = deskewLegOdomMsg.pose.pose.orientation.y;
        pose_stamped.pose.orientation.z = deskewLegOdomMsg.pose.pose.orientation.z;
        pose_stamped.pose.orientation.w = deskewLegOdomMsg.pose.pose.orientation.w;

        deskewLegOdomMsgPath.header.frame_id = "camera_init";
        deskewLegOdomMsgPath.header.stamp = ros::Time().fromSec(lidar_end_time);
        deskewLegOdomMsgPath.poses.push_back(pose_stamped);
        pubPath.publish(deskewLegOdomMsgPath);
    }

};




int main(int argc, char** argv)
{ 
    ros::init(argc, argv, "processTopicMsg");

    ProcessTopicsMsg MO;

    ROS_INFO("\033[1;32m----> Procee TopicsMsg Started.\033[0m");

    std::thread processTopicsMsgthread(&ProcessTopicsMsg::processLegOdomMsg, &MO);

    ros::spin();

    processTopicsMsgthread.join();

    return 0;
}